// 参数定义
const float r = /* 轮半径 */;
const float L = /* 心到轮心距离 */;
const float Kp = /* P 增益 */;
const float Ki = /* I 增益 */;
float integral_e = 0;

// 主循环（周期 e.g. 1ms~10ms）
void control_loop(float vx_cmd, float vy_cmd, float dt) {
    // 1) 读取四轮实际角速度
    float w1 = read_encoder(1); // 前左
    float w2 = read_encoder(2); // 前右
    float w3 = read_encoder(3); // 后右
    float w4 = read_encoder(4); // 后左

    // 2) 估算当前角速度 omega_z (正运动学第三行)
    // 根据新的角度和正转定义，omega_z = (r/(4*L))*(+w1 - w2 + w3 - w4)
    float omega_z = (r/(4*L))*(w1 - w2 + w3 - w4);

    // 3) PI 计算修正角速度
    integral_e += omega_z * dt;
    float omega_corr = -(Kp * omega_z + Ki * integral_e); // 负号表示抵消当前旋转趋势

    // 4) 逆运动学算四轮目标速度
    const float c45_neg = -0.70710678f; // cos(-135), cos(135)
    const float c45_pos =  0.70710678f; // cos(45), cos(-45)
    const float s45_neg = -0.70710678f; // sin(-135), sin(-45)
    const float s45_pos =  0.70710678f; // sin(135), sin(45)

    // wheel 1 (前左): theta1 = -135deg, sign1 = +1
    float w1_cmd = (vx_cmd*c45_neg + vy_cmd*s45_neg + omega_corr*L) / r;
    // wheel 2 (前右): theta2 = 135deg, sign2 = -1
    float w2_cmd = (vx_cmd*c45_neg + vy_cmd*s45_pos - omega_corr*L) / r;
    // wheel 3 (后右): theta3 = 45deg, sign3 = +1
    float w3_cmd = (vx_cmd*c45_pos + vy_cmd*s45_pos + omega_corr*L) / r;
    // wheel 4 (后左): theta4 = -45deg, sign4 = -1
    float w4_cmd = (vx_cmd*c45_pos + vy_cmd*s45_neg - omega_corr*L) / r;

    // 5) 下发电机速度指令
    set_motor_speed(1, w1_cmd);
    set_motor_speed(2, w2_cmd);
    set_motor_speed(3, w3_cmd);
    set_motor_speed(4, w4_cmd);
}